#ifndef A_LIBRARY_FILE_LIBRARY_H
#define A_LIBRARY_FILE_LIBRARY_H

#include <stdint.h>
#include <stdbool.h>

//——————————————————————————————————————进阶PID——————————————————————————————————————————————————————
typedef struct {
    float kp;                    // 比例系数 KP
    float kp2;                   // 非线性比例系数 KP2
    float kd;                    // 微分系数 KD
    float gkd;                   // 陀螺仪微分系数 GKD
    int pwm_max;                 // PWM最大值
    int pwm_min;                 // PWM最小值
    int left_motor_pwm;          // 左电机输出
    int right_motor_pwm;         // 右电机输出
    float ki_er;                 // 积分增益，即PID中的KI增益
    float ki_output;             // 存储积分项输出
    float DT;                    // 运算步长，例如0.01表示10ms
    float last_image_deviation;  // 存储上一次的误差信息
} SteeringParams;


// 初始化PID参数的函数  TODO
void SteeringParams_Init(SteeringParams* params);

// 进阶PID控制  TODO
void steering_control(SteeringParams * params,float image_deviation, float yaw_rate);

//——————————————————————————————————————进阶PID——————————————————————————————————————————————————————


//——————————————————————————————————————普通位置式PID——————————————————————————————————————————————————
// 定义PID参数和状态结构体
typedef struct {
    // PID控制器参数
    float kp;           // 比例增益
    float ki;           // 积分增益
    float kd;           // 微分增益
    float setpoint;     // 目标值
    float integral_max; // 积分限幅最大值
    float integral_min; // 积分限幅最小值
    float output_max;   // 输出限幅最大值
    float output_min;   // 输出限幅最小值

    // PID控制器状态
    float integral;     // 积分项
    float last_error;   // 上一次误差
} PIDController;

// 初始化函数 TODO
void PID_Init(PIDController *pid);

// 位置式PID计算函数  TODO
float PID_Compute(PIDController *pid, float measured_value, float dt);


//——————————————————————————————————————普通位置式PID——————————————————————————————————————————————————


//——————————————————————————————————————增量式PID——————————————————————————————————————————————————
// 定义增量式PID参数和状态结构体
typedef struct {
    // PID控制器参数
    float kp;           // 比例增益
    float ki;           // 积分增益
    float kd;           // 微分增益
    float setpoint;     // 目标值
    float integral_max; // 积分限幅最大值
    float integral_min; // 积分限幅最小值
    float output_max;   // 输出限幅最大值
    float output_min;   // 输出限幅最小值

    // PID控制器状态
    float integral;     // 积分项
    float last_error;   // 上一次误差
    float prev_error;   // 上上一次误差
    float last_output;  // 上一次输出
} IncrementalPID;

// 初始化增量式PID控制器  TODO
void IncrementalPID_Init(IncrementalPID *pid);

// 增量式pid  TODO
float IncrementalPID_Compute(IncrementalPID *pid, float measured_value);

//——————————————————————————————————————增量式PID——————————————————————————————————————————————————


//——————————————————————————————————————卡尔曼滤波模块——————————————————————————————————————————————————
// 卡尔曼滤波模块
typedef struct {
    float x_est;  // 估计误差
    float v_est;  // 估计变化率
    float p_est_00;  // 协方差矩阵[0,0]
    float p_est_01;  // 协方差矩阵[0,1]
    float p_est_10;  // 协方差矩阵[1,0]
    float p_est_11;  // 协方差矩阵[1,1]
    float q1;  // 过程噪声协方差（误差）
    float q2;  // 过程噪声协方差（变化率）
    float r;   // 测量噪声协方差
    float dt;  // 时间步长
    float prediction_horizon;    // 预测时间，越短其反应越灵敏，越快
} KalmanFilter;

// 卡尔曼滤波初始化  TODO
void init_kalman_filter(KalmanFilter* filter, float q1, float q2, float r, float dt);

// 卡尔曼滤波函数，模型不通用，不同模型需要不同的函数实现  TODO
float kalman_filter_predict(KalmanFilter* filter, float measurement);

//——————————————————————————————————————卡尔曼滤波模块——————————————————————————————————————————————————


//——————————————————————————————————————一阶低通滤波——————————————————————————————————————————————————
// 定义滤波器参数结构体
typedef struct {
    float alpha;       // 滤波系数
    float previous_output; // 上一次滤波后的输出值
} LowPassFilter;

// 函数简介     一阶低通滤波参数初始化   TODO
void LowPassFilter_Init(LowPassFilter *filter, float cutoff_frequency, float sample_time);  // 滤波器初始化

// 函数简介     一阶低通滤波   TODO
float LowPassFilter_Update(LowPassFilter *filter, float input);  // 执行滤波操作

//——————————————————————————————————————一阶低通滤波——————————————————————————————————————————————————

//——————————————————————————————————————查表用于tanh——————————————————————————————————————————————————

// 定义结构体：用于存储查表信息
typedef struct {
    float x_min;       // 输入范围最小值
    float x_max;       // 输入范围最大值
    int table_size;     // 实际使用的查表大小
    float tanh_table[1000]; // 静态分配的查表数据
} TanhTable;

// 函数简介     查表法初始化(用于计算tanh函数)   TODO
void init_tanh_table(TanhTable *table, float min, float max, int size);

// 函数简介     查表法计算tanh函数用于非线性缩放  TODO
float tanh_lookup(const TanhTable *table, float x);

//——————————————————————————————————————查表用于tanh——————————————————————————————————————————————————


//——————————————————————————————————————查表用于指数平滑——————————————————————————————————————————————————

typedef struct {
    float table[1000];  // 预计算表，存储 1 - exp(-x) 的值
    int table_size;            // 表的大小
    float sigma;              // 缩放因子 sigma
    float e_max;              // 误差的最大值
    float step;               // 表中每个索引对应的 x 步长 (e_max / sigma / (table_size - 1))
} ExpSmoothingTable;
//sigma越小 增长越快，调整更敏感   sigma越大 变化越平缓，调整更温和

// 函数简介     查表法初始化(用于计算指数平滑函数)  TODO
void init_exp_smoothing_table(ExpSmoothingTable *est, float sigma, float e_max, int table_size);

// 函数简介     查表法计算指数平滑函数用于非线性缩放  TODO
float exp_smoothing_lookup(const ExpSmoothingTable *est, float e,float kp);

//——————————————————————————————————————查表用于指数平滑——————————————————————————————————————————————————


//——————————————————————————————————————CAN报文字节处理——————————————————————————————————————————————————
// 函数简介     int类型拆成四个字节，设置参数的时候需要这么操作   TODO
void plit_int_to_bytes(int32_t value, uint8_t *data);

// 函数简介     高低字节融合整形   TODO
int16_t Merge_uint8(uint8_t high_byte,uint8_t low_byte);

//——————————————————————————————————————CAN报文字节处理——————————————————————————————————————————————————

//——————————————————————————————————————单位换算——————————————————————————————————————————————————
// rpm转换为rad/s  TODO
float rpm_to_rads(float rpm);

//——————————————————————————————————————单位换算——————————————————————————————————————————————————


//——————————————————————————————————————绝对值编码器处理(过零检测)——————————————————————————————————————————————————

typedef struct {
    bool first_reading ;      // 初始上电角度检测标志位
    int Actual_angle;         // 当前角度(编码器值)
    int last_angle;           // 储存上一次的角度(编码器值)
    float Actual_speed;       // 当前角速度
    int32_t raw_delta;        // 对角度误差进行取模运算，取最小路径，接收过零后的差值
    int32_t total_angle;      // 检测当前电机旋转的绝对角度，可以是任何圈数，且该值用于差值计算，作为检测的值
    int32_t over_zero_count;  // 过零的圈数，叠加角度
} Handle_encoder_zero;

// 函数简介     编码器过零检测结构体初始化  TODO
void Encoder_zero_init(Handle_encoder_zero *state);   // 编码器过零检测结构体初始化

//绝对编码器值处理,过零处理  TODO
void Handle_encoder(Handle_encoder_zero *state,uint8_t high_byte_angle,uint8_t low_byt_angle,uint8_t high_byte_speed,uint8_t low_byt_speed);

//——————————————————————————————————————绝对值编码器处理(过零检测)——————————————————————————————————————————————————


//——————————————————————————————————————查表用于COS/SIN计算——————————————————————————————————————————————————

// 定义结构体，用于保存查表法所需的参数
typedef struct {
    int table_size;       // 表的大小
    float step;          // 每个角度的步长（弧度）
    float cos_table[360]; // 静态数组，用于存储余弦值（固定大小为360）
} CosLookupTable;

// 初始化查表结构体
void init_cos_lookup(CosLookupTable* table, int size);

// 使用查表法计算余弦值
float lookup_cos(const CosLookupTable* table, float angle);

// 定义结构体，用于保存查表法所需的参数
typedef struct {
    int table_size;       // 表的大小
    float step;          // 每个角度的步长（弧度）
    float sin_table[360]; // 静态数组，用于存储余弦值（固定大小为360）
} SinLookupTable;

// 初始化查表结构体
void init_sin_lookup(SinLookupTable* table, int size);

// 使用查表法计算正弦值
float lookup_sin(const SinLookupTable* table, float angle);


//——————————————————————————————————————查表用于COS/SIN计算——————————————————————————————————————————————————



//——————————————————————————————————————查表用于ATAN2计算——————————————————————————————————————————————————

#define MIN_ATAN2_TABLE_SIZE 160 // 最小表大小
#define MAX_ATAN2_TABLE_SIZE 360 // 最大表大小

// 定义结构体，用于保存查表法所需的参数
typedef struct {
    int table_size; // 表的大小
    float step; // 每个比值的步长
    float atan_table[MAX_ATAN2_TABLE_SIZE]; // 静态数组，用于存储atan值
} Atan2LookupTable;


// 初始化查表结构体
void init_atan2_lookup(Atan2LookupTable* table, int table_size);

// 使用查表法计算atan2值
float lookup_atan2(const Atan2LookupTable* table, float y, float x);

//——————————————————————————————————————查表用于ATAN2计算——————————————————————————————————————————————————





















#endif //A_LIBRARY_FILE_LIBRARY_H
